/* -*- c++ -*- */
/*
 * Copyright 2004,2006 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <xstar_framer_sink.h>
#include <gr_io_signature.h>
#include <cstdio>
#include <stdexcept>

#define VERBOSE 0

inline void
xstar_framer_sink::enter_search()
{
  if (VERBOSE)
    fprintf(stderr, "@ enter_search\n");

  d_state = STATE_SEARCH;
}

inline void
xstar_framer_sink::enter_lost_sync_search()
{
  if (VERBOSE)
    fprintf(stderr, "@ enter_lost_sync_search\n");

  d_state = STATE_LOST_SYNC_SEARCH;
  d_sync_frame = 0;
}

inline void
xstar_framer_sink::enter_radio_header()
{
  if (VERBOSE)
    fprintf(stderr, "@ enter_radio_header\n");

  d_state = STATE_IN_RADIO_HEADER;
  d_subframe_byte = 0;
  d_subframe_byte_index = 0;
  d_subframelen = HEADER_LEN;
  d_subframelen_cnt = 0;
  d_numsubframe = 0;
}

inline void
xstar_framer_sink::enter_frame_header()
{
  if (VERBOSE)
    fprintf(stderr, "@ enter_frame_header\n");

  d_state = STATE_IN_FRAME_HEADER;
  d_subframe_byte = 0;
  d_subframe_byte_index = 0;
  d_subframelen = FRAME_HEADER_LEN;
  d_subframelen_cnt = 0;
  ++d_numsubframe;
}

inline void
xstar_framer_sink::enter_voice_frame()
{
  if (VERBOSE)
    fprintf(stderr, "@ enter_voice_frame\n");

  d_state = STATE_IN_VOICE_FRAME;
  d_subframelen = VOICE_FRAME_LEN;
}

inline void
xstar_framer_sink::enter_sync_frame()
{
  if (VERBOSE)
    fprintf(stderr, "@ enter_sync_frame\n");

  d_state = STATE_IN_SYNC_FRAME;
  d_subframelen = SYNC_FRAME_LEN;
}

inline void
xstar_framer_sink::enter_end_frame()
{
  if (VERBOSE)
    fprintf(stderr, "@ enter_end_frame\n");

  d_state = STATE_IN_END_FRAME;
  d_subframelen = END_FRAME_LEN;
}

xstar_framer_sink_sptr
xstar_make_framer_sink(gr_msg_queue_sptr target_queue)
{
  return xstar_framer_sink_sptr(new xstar_framer_sink(target_queue));
}


xstar_framer_sink::xstar_framer_sink(gr_msg_queue_sptr target_queue)
  : gr_sync_block ("framer_sink_xstar",
		   gr_make_io_signature (1, 1, sizeof(unsigned char)),
		   gr_make_io_signature (0, 0, 0)),
    d_target_queue(target_queue)
{
  enter_search();
}

xstar_framer_sink::~xstar_framer_sink ()
{
}

int
xstar_framer_sink::work (int noutput_items,
			 gr_vector_const_void_star &input_items,
			 gr_vector_void_star &output_items)
{
  const unsigned char *in = (const unsigned char *) input_items[0];
  int count=0;

  while (count < noutput_items) {
    if (d_state == STATE_SEARCH) {
      while (count < noutput_items) {
	if (in[count] & 0x2) {  // Found it, set up for header decode
	  enter_radio_header();
	  break;
	}
	count++;
      }
    } else if (d_state == STATE_LOST_SYNC_SEARCH) {
      while (count < noutput_items) {
	if (in[count] & 0x2) {  // Found radio header sync it, set up for header decode
	  enter_radio_header();
	  break;
	}

	d_sync_frame = d_sync_frame << 1 | in[count] & 0x1;

	fprintf(stderr, "%08x vs. %08x\n", d_sync_frame, 0x332CA98B);

	if (d_sync_frame == 0x332CA98B) {
	  if (VERBOSE)
	    fprintf(stderr, "Regained sync!\n");

	  enter_frame_header();
	  count++;
	  break;
	}
	count++;
      }
    } else {
      while (count < noutput_items) { // shift bits into bytes of subframe one at a time
	d_subframe_byte = (d_subframe_byte << 1) | (in[count++] & 0x1);
	if (d_subframe_byte_index++ == 7) { // byte is full so move to next byte
	  d_subframe[d_subframelen_cnt++] = d_subframe_byte;
	  d_subframe_byte_index = 0;

	  if (d_subframelen_cnt == d_subframelen) {

            if (d_state == STATE_IN_FRAME_HEADER) { // Type of subframe
              if (d_subframe[0] == SYNC_FRAME_TYPE) {
                enter_sync_frame();
                break;
              } else if (d_subframe[0] == VOICE_FRAME_TYPE) {
                enter_voice_frame();
                break;
              } else if (d_subframe[0] == END_FRAME_TYPE) {
                enter_end_frame();
                break;
              } else {
                fprintf(stderr,"Unknown subframe: %c (0x%x)\n", d_subframe[0], d_subframe[0]);
                enter_lost_sync_search();
                break;
              }
            }

            // subframe is complete
	    const unsigned char *payload = d_subframe + (d_state != STATE_IN_RADIO_HEADER);
	    int payload_len = d_subframelen_cnt - (d_state != STATE_IN_RADIO_HEADER);
	    unsigned char type = (d_state == STATE_IN_RADIO_HEADER) ? '0' : d_subframe[0];

	    if (VERBOSE)
	      fprintf(stderr, "<frame, type=%c, len=%02d, data=%-*s>\n", type, payload_len, payload_len, payload);

            if (d_state != STATE_IN_SYNC_FRAME) {
              // build a message
              gr_message_sptr msg = gr_make_message(type, 0, 0, payload_len);
              memcpy(msg->msg(), payload, payload_len);
              d_target_queue->insert_tail(msg);	// send it
              msg.reset();			// free it up
            }

            // Find next state
            if (type == END_FRAME_TYPE) {
              enter_search();
            } else {
              enter_frame_header();
            }
            break;
	  }
	}
      }
    }
  }   // while

  return noutput_items;
}

bool
xstar_framer_sink::set_postamble(
  const std::string &postamble)
{
    // TODO: Remove

  return true;
}

// vim: sts=2 sw=2 noet
